#ifndef _PID1_
#define _PID1_
#include <SetMotor_Code.h>
#include <GetData.h>
#include <GetMotorCode.h>


float Kp = 10, Ki = 0.05, Kd = 0;         
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;

extern unsigned long nextPID;
float previous_error = 0, previous_I = 0;         
 int left_motor_speed, right_motor_speed;
int initial_motor_speed = 0;           
static int ispidinit=0;
void read_sensor_values(void);                    
void calc_pid(void);                              
void motor_control(void);                        
 

void motorsWrite(int speedL, int speedR)
{
  if(speedL>=0)
  {
    SetMotor_Code(1, 0,speedL / 2.55);
  }
  else
  {
    SetMotor_Code(1, 2,-speedL / 2.55);
  }
  
  if(speedR>=0)
  {
    SetMotor_Code(2, 0,speedR / 2.55);
  }
  else
  {
    SetMotor_Code(2, 2,-speedR / 2.55);
  }
}

void read_sensor_values()
{
  if(initial_motor_speed>=0)
    {
      error=abs(GetMotorCode(1))-abs(GetMotorCode(2));
    }
    else
    {
      error=-(abs(GetMotorCode(1))-abs(GetMotorCode(2)));
    }
    
}
void calc_pid()
{
  if(!ispidinit)
  {
    nextPID += GetSysTimeMs();
    ispidinit=0;
  }
  if (GetSysTimeMs() >= nextPID) {
		
		nextPID += 1000;
	  }
  P = error;
  I = I + error;
  D = error - previous_error;
 
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  
  previous_error = error;
}
void motor_control()
{
  left_motor_speed = initial_motor_speed*2.55 - PID_value;
  right_motor_speed = initial_motor_speed*2.55 + PID_value;
  
  if(left_motor_speed < -255){
    left_motor_speed = -255;
  }
  if(left_motor_speed > 255){
    left_motor_speed = 255;
  }

  if(right_motor_speed < -255){
    right_motor_speed = -255;
  }
  if(right_motor_speed > 255){
    right_motor_speed = 255;
  }

  motorsWrite(left_motor_speed,right_motor_speed);
 //SetLCD5Char(0, 0, left_motor_speed, YELLOW, BLACK);
 //SetLCD5Char(0,32, right_motor_speed, YELLOW, BLACK);

//SetLCD5Char(110, 0, GetMotorCode(1), YELLOW, BLACK);
 //SetLCD5Char(110,32, GetMotorCode(2), YELLOW, BLACK);
 
} 

#endif

